#include "vehicle_linear2d2s.h"
#include "mpc.h"
#include <iostream>
#include <chrono>
#include "matplotlibcpp.hpp"
//#define printout ;
namespace plt = matplotlibcpp;
using namespace auto_ros::control;
using namespace LinearMpc;
int main()
{

	double speed = 10;
	//dyn_sys->discretization->setup mpc
	//speed_update_matrix->discreate->add_constraints or bound ->update_mpc->solve
	VehicleLinear2d2s test_c(1500, 48000, 48000, 1.015, 1.895, 1536.7, 7, 0.5);
#ifdef printout
	std::cout << " state_matrix is:" << std::endl
			  << test_c.state_matrix() << std::endl;
	std::cout << "control matrix is:" << std::endl
			  << test_c.control_matrix() << std::endl;
	std::cout << "distur matrix is:" << std::endl
			  << test_c.distur_matrix() << std::endl;
#endif
	test_c.speed_update_matrix(speed);
#ifdef printout
	std::cout << " 10m/s update state_matrix is:" << std::endl
			  << test_c.state_matrix() << std::endl;
	std::cout << "10m/s update control matrix is:" << std::endl
			  << test_c.control_matrix() << std::endl;
	std::cout << "10m/s update distur matrix is:" << std::endl
			  << test_c.distur_matrix() << std::endl;
#endif
	double rate = 100;
	std::shared_ptr<DiscreteLinearSys> test_dis = test_c.discretization(1 / rate);
#ifdef printout
	std::cout << "10m/s dis 0.01s dis state_matrix is:" << std::endl
			  << test_dis->state_matrix() << std::endl;
	std::cout << "10m/s dis 0.01s dis control matrix is:" << std::endl
			  << test_dis->control_matrix() << std::endl;
	std::cout << "10m/s dis 0.01s dis distur matrix is:" << std::endl
			  << test_dis->distur_matrix() << std::endl;
#endif

	int state_dim = 4;
	int control_dim = 1;
	int distur_dim = 1;
	int mpc_windows = 100;
	int max_xu_constraint = 1;
	int max_delta_x = 0;
	int max_delta_u = 1;

	int max_final_constraint = 0;
	Mpc test_mpc(test_dis, mpc_windows, max_xu_constraint, max_delta_x, max_delta_u, max_final_constraint);
	//test defualt hessian
#ifdef printout
	Eigen::MatrixXd temp_matrix = Eigen::MatrixXd(test_mpc.hessian());
	std::cout << "defualt hessian set is ok:" << std::endl;
	std::cout << temp_matrix << std::endl;
	std::cout << test_mpc.gradient().transpose() << std::endl;
	//test defualt constraint matrix

	temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
	std::cout << "defualt constriant_matrix is ok:" << std::endl;
	std::cout << temp_matrix << std::endl;
	std::cout << "default bound is:" << std::endl;
	std::cout << test_mpc.lowerBound().transpose() << std::endl;
	std::cout << test_mpc.upperBound().transpose() << std::endl;
#endif

	//add constriant
	/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
						   const std::vector<double> u_index, const std::vector<double> u_factor,
						   int constraint_index, double min, double max)*/
	std::cout << "max_tire_f_angle is:" << test_c.max_tire_f_angle() << std::endl;
	test_mpc.set_xu_affine({}, {}, {0}, {1}, 0, -test_c.max_tire_f_angle(), test_c.max_tire_f_angle());
	//set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
	test_mpc.set_delta_u(0, 0, -0.5 / rate * 10, 0.5 / rate * 10);
	/*	bool update_mpc(
		const Eigen::MatrixXd &q, const Eigen::MatrixXd &r,
		const Eigen::VectorXd &x0, const Eigen::MatrixXd &x_ref,
		const Eigen::MatrixXd &distur_ref); // dis_sys_ptr  with disturbance*/
	double max_angle = test_c.speed_update_max_tire_f_angle(speed);
	std::cout << "#################### max angle is:" << max_angle << std::endl;
	test_mpc.set_xu_bound(0, -max_angle, max_angle);
	auto start = std::chrono::high_resolution_clock::now();
	Eigen::MatrixXd q(state_dim, state_dim);
	q << 1, 0, 0, 0,
		0, 0.0, 0, 0,
		0, 0, 0.00, 0,
		0, 0, 0, 0;
	Eigen::MatrixXd r(control_dim, control_dim);
	r << 0.0;
	Eigen::VectorXd x0(state_dim);
	x0 << 0.2, 0, 0, 0;
	Eigen::VectorXd u_fb(control_dim);
	u_fb << 0.0;
	Eigen::VectorXd x_ref(state_dim);
	x_ref << 0.0, 0.0, 0.0, 0.0;
	Eigen::MatrixXd distur_ref(distur_dim, 1);
	distur_ref << 0.2;

//std::cout << "\033[31m x0################################ is: \033[0m" << x0 << std::endl;
#ifdef printout
	temp_matrix = Eigen::MatrixXd(test_mpc.hessian());
	std::cout << "update hessian is:" << std::endl;
	std::cout << temp_matrix << std::endl;
	std::cout << "update gradient is:" << std::endl;
	std::cout << test_mpc.gradient().transpose() << std::endl;
	//test defualt constraint matrix

	temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
	std::cout << "update constriant_matrix is ok:" << std::endl;
	std::cout << temp_matrix << std::endl;
	std::cout << "update bound is:" << std::endl;
	std::cout << test_mpc.lowerBound().transpose() << std::endl;
	std::cout << test_mpc.upperBound().transpose() << std::endl;
#endif
	int max_iter = 100;
	for (int i = 0; i < max_iter; i++)
	{
		if (!test_mpc.update_mpc(q, r, x0, u_fb, x_ref, distur_ref))
		{
			std::cout << "\033[31m mpc update failed \033[0m" << std::endl;
		}
		test_mpc.mpc_solve();
		u_fb = test_mpc.get_contorl_one_shot(0);
		std::cout << i << " u is:" << u_fb << std::endl;
		/*ContinuousLinearSys::integral(
	const uint segment, const double start_time, const double end_time,
	const Eigen::VectorXd x0, const Eigen::VectorXd u, const Eigen::VectorXd distur, Eigen::VectorXd xf) */
		Eigen::VectorXd xf(state_dim);
		test_c.integral(100, 0, 1 / rate, x0, u_fb, distur_ref, xf);
		std::cout << i << " x is:" << x0.transpose() << std::endl;
		x0 = xf;
	}
	auto end = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double, std::milli> tm = end - start; // 毫秒
	std::cout << "time: " << tm.count() << "ms" << std::endl;
	/*std::vector<double> state_vector = test_mpc.get_state_vector(0);
	for (int i = 0; i < state_vector.size(); i++)
	{
		std::cout << i << ":" << state_vector[i] << std::endl;
	}

	for (int i = 0; i < control_vector.size(); i++)
	{
		std::cout << i << "u :" << control_vector[i] << std::endl;
	}

	state_vector = test_mpc.get_state_vector(0);
	for (int i = 0; i < state_vector.size(); i++)
	{
		std::cout << i << " x:" << state_vector[i] << std::endl;
	}
	std::cout << test_mpc.get_state_one_shot(mpc_windows - 1) << std::endl;
	std::cout << test_mpc.get_contorl_one_shot(mpc_windows - 1) << std::endl;
	//plt::plot(control_vector);
	//plt::show();*/
}
